Merge pull request #8 from johnwason/pr/UpstreamChanges

Update to upstream changes

Author: johnwason

tesseract_python/swig/tesseract_common_python.i

@@ -41,6 +41,7 @@
 #include <tesseract_common/status_code.h>
 #include <tesseract_common/resource.h>
 #include <tesseract_common/manipulator_info.h>
+#include <tesseract_common/joint_state.h>
 
 %}
 
@@ -133,6 +134,7 @@ namespace tesseract_common
 %include "tesseract_common/status_code.h"
 %include "tesseract_common/resource.h"
 %include "tesseract_common/manipulator_info.h"
+%include "tesseract_common/joint_state.h"
 
 
 

tesseract_python/swig/tesseract_environment_python.i

@@ -67,6 +67,28 @@
 #define TESSERACT_ENVIRONMENT_CORE_PUBLIC
 %include "tesseract_environment/core/types.h"
 %include "tesseract_environment/core/commands.h"
+%include "tesseract_environment/core/command.h"
+%include "tesseract_environment/core/commands/add_allowed_collision_command.h"
+%include "tesseract_environment/core/commands/add_link_command.h"
+%include "tesseract_environment/core/commands/add_kinematics_information_command.h"
+%include "tesseract_environment/core/commands/add_scene_graph_command.h"
+%include "tesseract_environment/core/commands/change_default_contact_margin_command.h"
+%include "tesseract_environment/core/commands/change_joint_acceleration_limits_command.h"
+%include "tesseract_environment/core/commands/change_joint_origin_command.h"
+%include "tesseract_environment/core/commands/change_joint_position_limits_command.h"
+%include "tesseract_environment/core/commands/change_joint_velocity_limits_command.h"
+%include "tesseract_environment/core/commands/change_link_collision_enabled_command.h"
+%include "tesseract_environment/core/commands/change_link_origin_command.h"
+%include "tesseract_environment/core/commands/change_link_visibility_command.h"
+%include "tesseract_environment/core/commands/change_pair_contact_margin_command.h"
+%include "tesseract_environment/core/commands/move_joint_command.h"
+%include "tesseract_environment/core/commands/move_link_command.h"
+%include "tesseract_environment/core/commands/remove_allowed_collision_command.h"
+%include "tesseract_environment/core/commands/remove_allowed_collision_link_command.h"
+%include "tesseract_environment/core/commands/remove_joint_command.h"
+%include "tesseract_environment/core/commands/remove_link_command.h"
+%include "tesseract_environment/core/commands/replace_joint_command.h"
+
 %include "tesseract_environment/core/manipulator_manager.h"
 %include "tesseract_environment/core/state_solver.h"
 %include "tesseract_environment/core/environment.h"

tesseract_python/swig/tesseract_kinematics_python.i

@@ -58,26 +58,6 @@
 
 %template(pair_bool_matrix) std::pair<bool,Eigen::MatrixXd>;
 
-%extend tesseract_kinematics::ForwardKinematics {
-
-  bool calcJacobian(Eigen::MatrixXd& jacobian, const Eigen::Ref<const Eigen::VectorXd>& joint_angles) const
-  {
-    jacobian = Eigen::MatrixXd(6, joint_angles.rows());
-    bool res = $self->calcJacobian(jacobian, joint_angles);
-    
-    return res;
-  }
-
-  bool calcJacobian(Eigen::MatrixXd& jacobian, const Eigen::Ref<const Eigen::VectorXd>& joint_angles, const std::string& link_name) const
-  {
-    jacobian = Eigen::MatrixXd(6, joint_angles.rows());
-    bool res = $self->calcJacobian(jacobian, joint_angles, link_name);
-    return res;
-  }
-}
-
-%ignore calcJacobian;
-
 // tesseract_kinematics
 #define TESSERACT_KINEMATICS_CORE_PUBLIC
 #define TESSERACT_KINEMATICS_IKFAST_PUBLIC

tesseract_python/tests/tesseract_kinematics/test_kdl_kinematics.py

@@ -83,12 +83,11 @@ def run_inv_kin_test(inv_kin, fwd_kin):
     pose[2,3] = 1.306
 
     seed = np.array([-0.785398, 0.785398, -0.785398, 0.785398, -0.785398, 0.785398, -0.785398])
-    res, solutions = inv_kin.calcInvKin(tesseract_common.Isometry3d(pose),seed)
-    assert res
-
-    res, result = fwd_kin.calcFwdKin(solutions)
-    assert res
+    solutions = inv_kin.calcInvKin(tesseract_common.Isometry3d(pose),seed)
+    assert len(solutions) > 0
 
+    result = fwd_kin.calcFwdKin(solutions[0])
+    
     nptest.assert_almost_equal(pose,result.matrix(),decimal=3)
 
 def test_kdl_kin_chain_lma_inverse_kinematic():
@@ -117,7 +116,7 @@ def test_jacobian():
     jvals = np.array([-0.785398, 0.785398, -0.785398, 0.785398, -0.785398, 0.785398, -0.785398])
 
     link_name = "tool0"
-    res, jacobian = kin.calcJacobian(jvals,link_name)
-    assert res
+    jacobian = kin.calcJacobian(jvals,link_name)
+    
     assert jacobian.shape == (6,7)
 

tesseract_python/tests/tesseract_motion_planning/test_simple_planner.py

@@ -49,7 +49,7 @@ def test_interpolatestatewaypoint_jointcart_freespace():
     request.env_state = env.getCurrentState()
     fwd_kin = env.getManipulatorManager().getFwdKinematicSolver(manip_info.manipulator)
     wp1 = JointWaypoint(joint_names, np.zeros((7,),dtype=np.float64))
-    wp2 = CartesianWaypoint(fwd_kin.calcFwdKin(np.ones((7,),dtype=np.float64))[1])
+    wp2 = CartesianWaypoint(fwd_kin.calcFwdKin(np.ones((7,),dtype=np.float64)))
     instr1 = PlanInstruction(Waypoint(wp1), PlanInstructionType_START, "TEST_PROFILE", manip_info)
     instr2 = PlanInstruction(Waypoint(wp2), PlanInstructionType_FREESPACE, "TEST_PROFILE", manip_info)
 
@@ -63,5 +63,5 @@ def test_interpolatestatewaypoint_jointcart_freespace():
 
     mi = composite[-1].cast_const_MoveInstruction()
     last_position = mi.getWaypoint().cast_const_StateWaypoint().position
-    _, final_pose = fwd_kin.calcFwdKin(last_position)
+    final_pose = fwd_kin.calcFwdKin(last_position)
     assert wp2.isApprox(final_pose, 1e-3)