@@ -1518,276 +1518,6 @@ def get_path(self, end=None, start=None):
1518
1518
1519
1519
return path , n , tool
1520
1520
1521
- def fkine (
1522
- self ,
1523
- q : ArrayLike ,
1524
- end : Union [str , Link , Gripper , None ] = None ,
1525
- start : Union [str , Link , Gripper , None ] = None ,
1526
- tool : Union [ndarray , SE3 , None ] = None ,
1527
- include_base : bool = True ,
1528
- ) -> SE3 :
1529
- """
1530
- Forward kinematics
1531
-
1532
- :param q: Joint coordinates
1533
- :type q: ArrayLike
1534
- :param end: end-effector or gripper to compute forward kinematics to
1535
- :param start: the link to compute forward kinematics from
1536
- :param tool: tool transform, optional
1537
-
1538
- :return: The transformation matrix representing the pose of the
1539
- end-effector
1540
-
1541
- - ``T = robot.fkine(q)`` evaluates forward kinematics for the robot at
1542
- joint configuration ``q``.
1543
- **Trajectory operation**:
1544
-
1545
- If ``q`` has multiple rows (mxn), it is considered a trajectory and the
1546
- result is an ``SE3`` instance with ``m`` values.
1547
- .. note::
1548
- - For a robot with a single end-effector there is no need to
1549
- specify ``end``
1550
- - For a robot with multiple end-effectors, the ``end`` must
1551
- be specified.
1552
- - The robot's base tool transform, if set, is incorporated
1553
- into the result.
1554
- - A tool transform, if provided, is incorporated into the result.
1555
- - Works from the end-effector link to the base
1556
-
1557
- :references:
1558
- - Kinematic Derivatives using the Elementary Transform
1559
- Sequence, J. Haviland and P. Corke
1560
- """
1561
- return SE3 (
1562
- self .ets (start , end ).fkine (
1563
- q , base = self ._T , tool = tool , include_base = include_base
1564
- ),
1565
- check = False ,
1566
- )
1567
-
1568
- def jacob0 (
1569
- self ,
1570
- q : ArrayLike ,
1571
- end : Union [str , Link , Gripper , None ] = None ,
1572
- start : Union [str , Link , Gripper , None ] = None ,
1573
- tool : Union [ndarray , SE3 , None ] = None ,
1574
- ) -> ndarray :
1575
- r"""
1576
- Manipulator geometric Jacobian in the base frame
1577
-
1578
- :param q: Joint coordinate vector
1579
- :type q: ArrayLike
1580
- :param end: the particular link or gripper whose velocity the Jacobian
1581
- describes, defaults to the end-effector if only one is present
1582
- :param start: the link considered as the base frame, defaults to the robots's base frame
1583
- :param tool: a static tool transformation matrix to apply to the
1584
- end of end, defaults to None
1585
-
1586
- :return J: Manipulator Jacobian in the base frame
1587
-
1588
- - ``robot.jacobo(q)`` is the manipulator Jacobian matrix which maps
1589
- joint velocity to end-effector spatial velocity expressed in the
1590
- end-effector frame.
1591
-
1592
- End-effector spatial velocity :math:`\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T`
1593
- is related to joint velocity by :math:`{}^{E}\!\nu = \mathbf{J}_m(q) \dot{q}`.
1594
-
1595
- Example:
1596
- .. runblock:: pycon
1597
- >>> import roboticstoolbox as rtb
1598
- >>> puma = rtb.models.ETS.Puma560()
1599
- >>> puma.jacobe([0, 0, 0, 0, 0, 0])
1600
8000
-
1601
- .. warning:: This is the geometric Jacobian as described in texts by
1602
- Corke, Spong etal., Siciliano etal. The end-effector velocity is
1603
- described in terms of translational and angular velocity, not a
1604
- velocity twist as per the text by Lynch & Park.
1605
-
1606
- .. warning:: ``start`` and ``end`` must be on the same branch,
1607
- with ``start`` closest to the base.
1608
- """ # noqa
1609
- return self .ets (start , end ).jacob0 (q , tool = tool )
1610
-
1611
- def jacobe (
1612
- self ,
1613
- q : ArrayLike ,
1614
- end : Union [str , Link , Gripper , None ] = None ,
1615
- start : Union [str , Link , Gripper , None ] = None ,
1616
- tool : Union [ndarray , SE3 , None ] = None ,
1617
- ) -> ndarray :
1618
- r"""
1619
- Manipulator geometric Jacobian in the end-effector frame
1620
-
1621
- :param q: Joint coordinate vector
1622
- :type q: ArrayLike
1623
- :param end: the particular link or Gripper whose velocity the Jacobian
1624
- describes, defaults to the end-effector if only one is present
1625
- :param start: the link considered as the base frame, defaults to the robots's base frame
1626
- :param tool: a static tool transformation matrix to apply to the
1627
- end of end, defaults to None
1628
-
1629
- :return J: Manipulator Jacobian in the end-effector frame
1630
-
1631
- - ``robot.jacobe(q)`` is the manipulator Jacobian matrix which maps
1632
- joint velocity to end-effector spatial velocity expressed in the
1633
- end-effector frame.
1634
- End-effector spatial velocity :math:`\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T`
1635
- is related to joint velocity by :math:`{}^{E}\!\nu = \mathbf{J}_m(q) \dot{q}`.
1636
-
1637
- Example:
1638
- .. runblock:: pycon
1639
- >>> import roboticstoolbox as rtb
1640
- >>> puma = rtb.models.ETS.Puma560()
1641
- >>> puma.jacobe([0, 0, 0, 0, 0, 0])
1642
-
1643
- .. warning:: This is the **geometric Jacobian** as described in texts by
1644
- Corke, Spong etal., Siciliano etal. The end-effector velocity is
1645
- described in terms of translational and angular velocity, not a
1646
- velocity twist as per the text by Lynch & Park.
1647
-
1648
- .. warning:: ``start`` and ``end`` must be on the same branch,
1649
- with ``start`` closest to the base.
1650
- """ # noqa
1651
- return self .ets (start , end ).jacobe (q , tool = tool )
1652
-
1653
- def hessian0 (
1654
- self ,
1655
- q : Union [ArrayLike , None ] = None ,
1656
- end : Union [str , Link , Gripper , None ] = None ,
1657
- start : Union [str , Link , Gripper , None ] = None ,
1658
- J0 : Union [ndarray , None ] = None ,
1659
- tool : Union [ndarray , SE3 , None ] = None ,
1660
- ) -> ndarray :
1661
- r"""
1662
- Manipulator Hessian
1663
-
1664
- The manipulator Hessian tensor maps joint acceleration to end-effector
1665
- spatial acceleration, expressed in the world-coordinate frame. This
1666
- function calulcates this based on the ETS of the robot. One of J0 or q
1667
- is required. Supply J0 if already calculated to save computation time
1668
-
1669
- :param q: The joint angles/configuration of the robot (Optional,
1670
- if not supplied will use the stored q values).
1671
- :type q: ArrayLike
1672
- :param end: the final link/Gripper which the Hessian represents
1673
- :param start: the first link which the Hessian represents
1674
- :param J0: The manipulator Jacobian in the 0 frame
1675
- :param tool: a static tool transformation matrix to apply to the
1676
- end of end, defaults to None
1677
-
1678
- :return: The manipulator Hessian in 0 frame
1679
-
1680
- This method computes the manipulator Hessian in the base frame. If
1681
- we take the time derivative of the differential kinematic relationship
1682
- .. math::
1683
- \nu &= \mat{J}(\vec{q}) \dvec{q} \\
1684
- \alpha &= \dmat{J} \dvec{q} + \mat{J} \ddvec{q}
1685
- where
1686
- .. math::
1687
- \dmat{J} = \mat{H} \dvec{q}
1688
- and :math:`\mat{H} \in \mathbb{R}^{6\times n \times n}` is the
1689
- Hessian tensor.
1690
-
1691
- The elements of the Hessian are
1692
- .. math::
1693
- \mat{H}_{i,j,k} = \frac{d^2 u_i}{d q_j d q_k}
1694
- where :math:`u = \{t_x, t_y, t_z, r_x, r_y, r_z\}` are the elements
1695
- of the spatial velocity vector.
1696
- Similarly, we can write
1697
- .. math::
1698
- \mat{J}_{i,j} = \frac{d u_i}{d q_j}
1699
-
1700
- :references:
1701
- - Kinematic Derivatives using the Elementary Transform
1702
- Sequence, J. Haviland and P. Corke
1703
- """
1704
- return self .ets (start , end ).hessian0 (q , J0 = J0 , tool = tool )
1705
-
1706
- def hessiane (
1707
- self ,
1708
- q : Union [ArrayLike , None ] = None ,
1709
- end : Union [str , Link , Gripper , None ] = None ,
1710
- start : Union [str , Link , Gripper , None ] = None ,
1711
- Je : Union [ndarray , None ] = None ,
1712
- tool : Union [ndarray , SE3 , None ] = None ,
1713
- ) -> ndarray :
1714
- r"""
1715
- Manipulator Hessian
1716
-
1717
- The manipulator Hessian tensor maps joint acceleration to end-effector
1718
- spatial acceleration, expressed in the ee frame. This
1719
- function calulcates this based on the ETS of the robot. One of Je or q
1720
- is required. Supply Je if already calculated to save computation time
1721
-
1722
- :param q: The joint angles/configuration of the robot (Optional,
1723
- if not supplied will use the stored q values).
1724
- :type q: ArrayLike
1725
- :param end: the final link/Gripper which the Hessian represents
1726
- :param start: the first link which the Hessian represents
1727
- :param Je: The manipulator Jacobian in the ee frame
1728
- :param tool: a static tool transformation matrix to apply to the
1729
- end of end, defaults to None
1730
-
1731
- :return: The manipulator Hessian in ee frame
1732
-
1733
- This method computes the manipulator Hessian in the ee frame. If
1734
- we take the time derivative of the differential kinematic relationship
1735
- .. math::
1736
- \nu &= \mat{J}(\vec{q}) \dvec{q} \\
1737
- \alpha &= \dmat{J} \dvec{q} + \mat{J} \ddvec{q}
1738
- where
1739
- .. math::
1740
- \dmat{J} = \mat{H} \dvec{q}
1741
- and :math:`\mat{H} \in \mathbb{R}^{6\times n \times n}` is the
1742
- Hessian tensor.
1743
-
1744
- The elements of the Hessian are
1745
- .. math::
1746
- \mat{H}_{i,j,k} = \frac{d^2 u_i}{d q_j d q_k}
1747
- where :math:`u = \{t_x, t_y, t_z, r_x, r_y, r_z\}` are the elements
1748
- of the spatial velocity vector.
1749
- Similarly, we can write
1750
- .. math::
1751
- \mat{J}_{i,j} = \frac{d u_i}{d q_j}
1752
-
1753
- :references:
1754
- - Kinematic Derivatives using the Elementary Transform
1755
- Sequence, J. Haviland and P. Corke
1756
- """
1757
- return self .ets (start , end ).hessiane (q , Je = Je , tool = tool )
1758
-
1759
- def partial_fkine0 (
1760
- self ,
1761
- q : ArrayLike ,
1762
- n : int = 3 ,
1763
- end : Union [str , Link , Gripper , None ] = None ,
1764
- start : Union [str , Link , Gripper , None ] = None ,
1765
- ):
1766
- r"""
1767
- Manipulator Forward Kinematics nth Partial Derivative
1768
-
1769
- The manipulator Hessian tensor maps joint acceleration to end-effector
1770
- spatial acceleration, expressed in the ee frame. This
1771
- function calulcates this based on the ETS of the robot. One of Je or q
1772
- is required. Supply Je if already calculated to save computation time
1773
-
1774
- :param q: The joint angles/configuration of the robot (Optional,
1775
- if not supplied will use the stored q values).
1776
- :type q: ArrayLike
1777
- :param end: the final link/Gripper which the Hessian represents
1778
- :param start: the first link which the Hessian represents
1779
- :param tool: a static tool transformation matrix to apply to the
1780
- end of end, defaults to None
1781
-
1782
- :return: The nth Partial Derivative of the forward kinematics
1783
-
1784
- :references:
1785
- - Kinematic Derivatives using the Elementary Transform
1786
- Sequence, J. Haviland and P. Corke
1787
- """
1788
-
1789
- return self .ets (start , end ).partial_fkine0 (q , n = n )
1790
-
1791
1521
def link_collision_damper (
1792
1522
self ,
1793
1523
shape ,
0 commit comments